﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;

namespace YBOT_Field_Control
{
    public class Field_Control
    {
        private OneWire fc1w;               //Create 1-wire object
        public XBee fcXB;                  //Create xb object
        public int maxTowerNum = 8;       //Max number of game towers including control board
        public int maxNodeNum = 8;
        /// <summary>
        /// state = Last Known State of Node (written or read)
        /// latchState = Current Latch State of Node
        /// writtenState = Last Written State of Node
        /// readState = Last Read State of Node
        /// </summary>
        public struct Node { public byte state; public byte latchSate; public byte writtenState; public byte readState;};
        public Node[] nodeState = new Node[20];

        #region Constructors

        public Field_Control()
        {
            this.fc1w = new OneWire(maxTowerNum);
            this.fcXB = new XBee();
        }

        public Field_Control(int _maxTowerNum)
        {
            this.maxTowerNum = _maxTowerNum;
            this.fc1w = new OneWire(maxTowerNum);
            this.fcXB = new XBee();
        }

        public Field_Control(OneWire _ow, XBee _xb)
        {
            this.fc1w = _ow;
            this.fcXB = _xb;
        }

        public Field_Control(int _maxTowerNum, OneWire _ow, XBee _xb)
        {
            this.maxTowerNum = _maxTowerNum;
            this.fc1w = _ow;
            this.fcXB = _xb;
        }

        #endregion

        /// <summary>
        /// Starts all communications with field
        /// </summary>
        public void StartUp()
        {
            this.fc1w.HubAllMainsOn();  //Turn on 1-wire hub's mains
        }

        /// <summary>
        /// Turns off field and field communications
        /// </summary>
        public void ShutDown()
        {
            this.FieldAllOff();     //Turn off all pins on the field
            this.fc1w.HubAllOff();  //Turn off all hubs
        }

        /// <summary>
        /// Writes given pin's state to given tower
        /// </summary>
        /// <param name="node">Node's Number</param>
        /// <param name="pin">Pin Number</param>
        /// <param name="state">true = on; false = off</param>
        public void PinWrite(int node, int pin, bool state)
        {
            this.fc1w.PinWrite(pin, node, state);                       //Write node's pin to given state
            this.nodeState[node].state = this.fc1w.owNode[node].stat;   //Write current state to node array
            this.nodeState[node].writtenState = this.fc1w.owNode[node].written;   //Write current state to node array
        }

        /// <summary>
        /// Write given state to node
        /// </summary>
        /// <param name="node">Node Number</param>
        /// <param name="state">byte to be written 1-on 0-off</param>
        public void WriteNodeState(int node, byte state)
        {
            this.fc1w.Set_NodeState(state, node);   //Write state to node
            this.nodeState[node].state = state;     //Write current state to node array
            this.nodeState[node].writtenState = this.fc1w.owNode[node].written;   //Write current state to node array
        }

        /// <summary>
        /// Check the state of given pin on given node
        /// From the last read or written state
        /// </summary>
        /// <param name="node">node number</param>
        /// <param name="pin">pin number</param>
        /// <returns>returns pin's state true=on, false=off</returns>
        public bool PinRead(int node, int pin)
        {
            byte _currentState = this.nodeState[node].state;    //Set _currentState to current nodes state

            byte _bitValue = 0;

            if (pin != 0)                                       //If pin is not zero
            {
                pin -= 1;                                       // Pin was selected with "people" number. Processing needs bit position (1 less).
                _bitValue = (byte)Math.Pow(2.0, pin);           // This returns the value of the bit positon
            }
            if ((_currentState & _bitValue) != _bitValue)       // the Pin is off
            {
                return false;
            }
            else
            {
                return true;                                    // the Pin must be on
            }
        }

        /// <summary>
        /// Check the given pin state or latch state on given node
        /// From the last read or written state
        /// </summary>
        /// <param name="node">node number</param>
        /// <param name="pin">pin number</param>
        /// <param name="latch">true = latch status; false = pin's status</param>
        /// <returns>returns pin's state or latch status</returns>
        public bool PinRead(int node, int pin, bool latch)
        {
            byte pinStatus;

            if (!latch)
            {
                this.nodeState[node].readState = this.fc1w.owNode[node].stat;   //Set state to node array
                this.nodeState[node].state = this.nodeState[node].readState;    //Get state and save in node array
                pinStatus = nodeState[node].state;
            }
            else 
            {
                this.nodeState[node].latchSate = this.fc1w.owNode[node].latch;   //set latch state in node array
                pinStatus = nodeState[node].latchSate;
            }

            byte bitValue = 0;
            if (pin != 0)
            {
                pin -= 1;                                     // pin was selected with "people" number. Processing needs bit position (1 less).
                bitValue = (byte)Math.Pow(2.0, pin);          // This returns the value of the bit positon
            }
            if ((pinStatus & bitValue) != bitValue)   // the pin is off
            {
                return false;
            }
            else
            {
                return true;                           // the pin must be on
            }
        }

        /// <summary>
        /// Read current state of given node
        /// </summary>
        /// <param name="node">node number</param>
        /// <returns>returns current state as byte 1=on, 2=off</returns>
        public byte ReadState(int node)
        {
            this.nodeState[node].readState = this.fc1w.Status(node);    //Get state and save in node array
            this.nodeState[node].state = this.nodeState[node].readState;    //Get state and save in node array
            return this.nodeState[node].readState;                           //return state of node
        }

        /// <summary>
        /// Read current State or Latch State of node
        /// </summary>
        /// <param name="node">node number</param>
        /// <param name="latch">true = latch status; false = current state</param>
        /// <returns>returns state/latch state of node as byte 1=on, 0=off</returns>
        public byte ReadState(int node, bool latch)
        {
            byte _state = this.fc1w.Status(node, latch);                //Get status of node
            if (!latch)
            {
                this.nodeState[node].readState = this.fc1w.owNode[node].stat;   //Write status of node to array
                this.nodeState[node].state = this.nodeState[node].readState;    //Get state and save in node array
            }
            else
            {
                this.nodeState[node].latchSate = this.fc1w.owNode[node].latch;   //set latch state in node array
            }
            return _state;                                              //Return state
        }

        /// <summary>
        /// Reset given node's latch status
        /// </summary>
        /// <param name="node"></param>
        public void LatchReset(int node)
        {
            this.fc1w.ResetActivityLatch(node);                             //reset nodes latch status
            this.nodeState[node].latchSate = this.fc1w.owNode[node].latch;  //set latch state in node array
        }

        /// <summary>
        /// Write all pins on or off for given node
        /// </summary>
        /// <param name="node">node number</param>
        /// <param name="state">true = on; false = off</param>
        public void AllPinsWrite(int node, bool state)
        {
            byte _state;
            if (!state) _state = 0;     //All off
            else _state = 0xFF;         //All on

            this.WriteNodeState(node, _state);   //Set node to given state
            this.nodeState[node].state = this.fc1w.owNode[node].stat;   //Write state to node array
            this.nodeState[node].writtenState = this.fc1w.owNode[node].stat;   //Write current state to node array
        }

        /// <summary>
        /// Turn on given light on given node and not change state of other pins
        /// </summary>
        /// <param name="node">Node number</param>
        /// <param name="lightPin">Light's pin number</param>
        /// <param name="state">true = on; false = off</param>
        public void Light(int node, int lightPin, bool state)
        {
            byte _currentState = this.nodeState[node].writtenState;            //Set _currentState to node's state

            lightPin -= 1;                                              // Pin was selected with "people" number. Processing needs bit number (1 less).
            byte _bitValue = (byte)Math.Pow(2.0, lightPin);             // This returns the value of the bit positon

            if (state) _currentState = (byte)(_currentState | _bitValue);   // Change selected Pin but maintain other Pins status
            else _currentState = (byte)(_currentState & ~_bitValue);        // Change selected Pin but maintain other Pins status

            this.WriteNodeState(node, _currentState);                        //Write new state to node keeping buttons from latching on

            this.nodeState[node].state = this.fc1w.owNode[node].stat;   //Record node's state
            this.nodeState[node].writtenState = _currentState;
        }

        /// <summary>
        /// Turns on multiple lights at the same time and not change state of other pins
        /// </summary>
        /// <param name="node">Node number</param>
        /// <param name="red">true = on, false = off</param>
        /// <param name="green">true = on, false = off</param>
        /// <param name="blue">true = on, false = off</param>
        /// <param name="yellow">true = on, false = off</param>
        public void Light(int node, bool red, bool green, bool blue, bool yellow)
        {
            byte _state = 0;

            int r = (int)pin.red;
            int g = (int)pin.green;
            int b = (int)pin.blue;
            int y = (int)pin.yellow;

            if (red) _state = (byte)(_state | (byte)(Math.Pow(2.0, (r -= 1))));
            if (green) _state = (byte)(_state | (byte)(Math.Pow(2.0, (g -= 1))));
            if (blue) _state = (byte)(_state | (byte)(Math.Pow(2.0, (b -= 1))));
            if (yellow) _state = (byte)(_state | (byte)(Math.Pow(2.0, (y -= 1))));

            this.WriteNodeState(node, _state);
        }

        /// <summary>
        /// Write all light pins to given state and not change state of other pins
        /// </summary>
        /// <param name="state">true = on ; false = off</param>
        public void AllLights(bool state)
        {
            for (int n = 0; n <= this.maxNodeNum; n++)                 //For all towers
            {
                if (n != (int)pin.controlBoard)                             //If the node isn't the control board
                {
                    byte _currentState = this.nodeState[n].state;       //Set _current state = node's state

                    if (state)                                          //If on
                    {
                        _currentState = (byte)(_currentState | 0XF);    //Set all light pins on
                        this.WriteNodeState(n, _currentState);               //Write state to node
                    }
                    else
                    {
                        _currentState = (byte)(_currentState & ~0XF);   //Set all light pins off
                        this.WriteNodeState(n, _currentState);               //Write state to node
                    }
                    this.nodeState[n].state = this.fc1w.owNode[n].stat; //Write state to node array
                    this.nodeState[n].writtenState = this.fc1w.owNode[n].written;   //Write current state to node array
                }
            }
        }

        /// <summary>
        /// Turn off all field components
        /// </summary>
        public void FieldAllOff()
        {
            for (int n = 0; n <= maxTowerNum; n++)                      //For all nodes
            {
                this.AllPinsWrite(n, false);                            //Write all pins off
                this.nodeState[n].state = this.fc1w.owNode[n].stat;     //Write stae in node array
                this.nodeState[n].writtenState = this.fc1w.owNode[n].written;   //Write current state to node array
            }
        }

        /// <summary>
        /// Clear all node tower latches
        /// </summary>
        public void AllTowersLatchReset()
        {
            for (int i = 0; i <= this.maxTowerNum; i++)
            {
                if (i != (int)pin.controlBoard) this.fc1w.ResetActivityLatch(i);
                this.nodeState[i].latchSate = this.fc1w.owNode[i].latch;
            }
        }

        /// <summary>
        /// Rind Field Bell
        /// </summary>
        public void RingBell()
        {
            this.PinWrite((int)pin.controlBoard, (int)pin.bell, true);
            this.PinWrite((int)pin.controlBoard, (int)pin.bell, false);
            Thread.Sleep(300);
            this.PinWrite((int)pin.controlBoard, (int)pin.bell, true);
            this.PinWrite((int)pin.controlBoard, (int)pin.bell, false);
        }

        /// <summary>
        /// Sound Buzzer
        /// </summary>
        public void SoundBuzzer()
        {
            this.PinWrite((int)pin.controlBoard, (int)pin.buzzer, true);
            Thread.Sleep(750);
            this.PinWrite((int)pin.controlBoard, (int)pin.buzzer, false);
        }

        /// <summary>
        /// Turn on both teams' transmitters to automode or manualmode
        /// </summary>
        /// <param name="autonomousState">true = automode, false = manual mode</param>
        /// <param name="transmitterState">true = controllers on, false = controllers off</param>
        public void RobotTransmitters(bool autonomousState, bool transmitterState)
        {
            if (transmitterState == true)   //If transmitters on
            {
                if (autonomousState)
                {
                    //Turn on Robot Transmiters Auto Mode
                    this.PinWrite((int)pin.redTeam_Node, (int)pin.transmitterRelay, true);
                    this.PinWrite((int)pin.greenTeam_Node, (int)pin.transmitterRelay, true);
                }
                else
                {
                    //Enter Manual Mode
                    this.PinWrite((int)pin.redTeam_Node, (int)pin.auto_driverRelay, true);
                    this.PinWrite((int)pin.redTeam_Node, (int)pin.transmitterRelay, true);
                    this.PinWrite((int)pin.greenTeam_Node, (int)pin.auto_driverRelay, true);
                    this.PinWrite((int)pin.greenTeam_Node, (int)pin.transmitterRelay, true);
                }
            }
            else
            {
                //Else turn off controller
                this.PinWrite((int)pin.redTeam_Node, (int)pin.transmitterRelay, false);
                this.PinWrite((int)pin.redTeam_Node, (int)pin.auto_driverRelay, false);
                this.PinWrite((int)pin.greenTeam_Node, (int)pin.transmitterRelay, false);
                this.PinWrite((int)pin.greenTeam_Node, (int)pin.auto_driverRelay, false);
            }
        }

        /// <summary>
        /// Turn on team's transmitters to automode or manualmode
        /// </summary>
        /// <param name="team">1 = red, 2 = green</param>
        /// <param name="autonomousState">true = automode, false = manual mode</param>
        /// <param name="transmitterState">true = controllers on, false = controllers off</param>
        public void RobotTransmitters(int team, bool autonomousState, bool transmitterState)
        {
            if (transmitterState == true)   //If transmitters on
            {
                if (autonomousState)
                {
                    //Turn on Robot Transmiters Auto Mode
                    if (team == 1) this.PinWrite((int)pin.redTeam_Node, (int)pin.transmitterRelay, true);
                    if (team == 2) this.PinWrite((int)pin.greenTeam_Node, (int)pin.transmitterRelay, true);
                }
                else
                {
                    //Enter Manual Mode
                    if (team == 1)
                    {
                        this.PinWrite((int)pin.redTeam_Node, (int)pin.auto_driverRelay, true);
                        this.PinWrite((int)pin.redTeam_Node, (int)pin.transmitterRelay, true);
                    }
                    if (team == 2)
                    {
                        this.PinWrite((int)pin.greenTeam_Node, (int)pin.auto_driverRelay, true);
                        this.PinWrite((int)pin.greenTeam_Node, (int)pin.transmitterRelay, true);
                    }
                }
            }
            else
            {
                //Else turn off controller
                if (team == 1)
                {
                    this.PinWrite((int)pin.redTeam_Node, (int)pin.transmitterRelay, false);
                    this.PinWrite((int)pin.redTeam_Node, (int)pin.auto_driverRelay, false);
                }
                if (team == 2)
                {
                    this.PinWrite((int)pin.greenTeam_Node, (int)pin.transmitterRelay, false);
                    this.PinWrite((int)pin.greenTeam_Node, (int)pin.auto_driverRelay, false);
                }
            }
        }

    }
}
